#pragma once

#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <pcl/common/common.h>
#include <pcl/common/transforms.h>
#include <pcl/filters/voxel_grid.h>

#include "Estimator.h"

// gstam
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/Values.h>

// 6D位姿点云结构定义
struct PointXYZIRPYT {
    PCL_ADD_POINT4D
    PCL_ADD_INTENSITY;
    float roll;
    float pitch;
    float yaw;
    double time;
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;

POINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZIRPYT, (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(float, roll, roll)(float, pitch, pitch)(
                                                     float, yaw, yaw)(double, time, time))

typedef PointXYZIRPYT PointTypePose;
typedef pcl::PointCloud<PointTypePose> CloudTypePose;
typedef pcl::PointCloud<PointTypePose>::Ptr CloudPtrPose;

#include "sc/Scancontext.h"
inline SCManager scManager;

inline bool EN_SUBMAP = false, EN_LOOP = false, EN_GTSAM = false;
inline bool startFlag = true;  // 主线程是否在运行
inline bool aLoopIsClosed = false;

inline ros::Publisher pubPathUpdate;

inline std::vector<CloudPtr> surfCloudKeyFrames;  // 历史所有关键帧的平面点集合（降采样）

inline CloudPtr cloudKeyPoses3D(new CloudType());                              // 历史关键帧位姿（位置）
inline CloudPtrPose cloudKeyPoses6D(new CloudTypePose());                      // 历史关键帧位姿
inline CloudPtrPose fastlio_unoptimized_cloudKeyPoses6D(new CloudTypePose());  //  存储fastlio 未优化的位姿

inline pcl::VoxelGrid<PointType> downSizeFilterICP;

inline float transformTobeMapped[6];  //  当前帧的位姿(world系下)

inline std::mutex mtx;

inline gtsam::NonlinearFactorGraph gtSAMgraph;

// 更新里程计轨迹
void updatePath(const PointTypePose& pose_in);

// 对点云cloudIn进行变换transformIn，返回结果点云， 修改liosam, 考虑到外参的表示
CloudPtr transformPointCloud(CloudPtr cloudIn, PointTypePose* transformIn);

// 位姿格式变换
inline gtsam::Pose3 pclPointTogtsamPose3(PointTypePose thisPoint) {
    return gtsam::Pose3(gtsam::Rot3::RzRyRx(double(thisPoint.roll), double(thisPoint.pitch), double(thisPoint.yaw)),
                        gtsam::Point3(double(thisPoint.x), double(thisPoint.y), double(thisPoint.z)));
}
// 位姿格式变换
inline gtsam::Pose3 trans2gtsamPose(float transformIn[]) {
    return gtsam::Pose3(gtsam::Rot3::RzRyRx(transformIn[0], transformIn[1], transformIn[2]), gtsam::Point3(transformIn[3], transformIn[4], transformIn[5]));
}
// Eigen格式的位姿变换
inline Eigen::Affine3f pclPointToAffine3f(PointTypePose thisPoint) {
    return pcl::getTransformation(thisPoint.x, thisPoint.y, thisPoint.z, thisPoint.roll, thisPoint.pitch, thisPoint.yaw);
}
// Eigen格式的位姿变换
inline Eigen::Affine3f trans2Affine3f(float transformIn[]) {
    return pcl::getTransformation(transformIn[3], transformIn[4], transformIn[5], transformIn[0], transformIn[1], transformIn[2]);
}
// 位姿格式变换
inline PointTypePose trans2PointTypePose(float transformIn[]) {
    PointTypePose thisPose6D;
    thisPose6D.x = transformIn[3];
    thisPose6D.y = transformIn[4];
    thisPose6D.z = transformIn[5];
    thisPose6D.roll = transformIn[0];
    thisPose6D.pitch = transformIn[1];
    thisPose6D.yaw = transformIn[2];
    return thisPose6D;
}
// 发布thisCloud，返回thisCloud对应msg格式
inline sensor_msgs::PointCloud2 publishCloud(ros::Publisher* thisPub, CloudPtr thisCloud, ros::Time thisStamp, std::string thisFrame) {
    sensor_msgs::PointCloud2 tempCloud;
    pcl::toROSMsg(*thisCloud, tempCloud);
    tempCloud.header.stamp = thisStamp;
    tempCloud.header.frame_id = thisFrame;
    if (thisPub->getNumSubscribers() != 0) thisPub->publish(tempCloud);
    return tempCloud;
}
// 点到坐标系原点距离
inline float pointDistance(PointType p) { return sqrt(p.x * p.x + p.y * p.y + p.z * p.z); }
// 两点之间距离
inline float pointDistance(PointType p1, PointType p2) {
    return sqrt((p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y) + (p1.z - p2.z) * (p1.z - p2.z));
}
//  eulerAngle 2 Quaterniond
inline Eigen::Quaterniond EulerToQuat(float roll_, float pitch_, float yaw_) {
    Eigen::Quaterniond q;  //   四元数 q 和 -q 是相等的
    Eigen::AngleAxisd roll(double(roll_), Eigen::Vector3d::UnitX());
    Eigen::AngleAxisd pitch(double(pitch_), Eigen::Vector3d::UnitY());
    Eigen::AngleAxisd yaw(double(yaw_), Eigen::Vector3d::UnitZ());
    q = yaw * pitch * roll;
    q.normalize();
    return q;
}

// 将更新的pose赋值到 transformTobeMapped
void getCurPose(state_output cur_state);

void publish_path_update(const ros::Publisher pubPath);
#include <Eigen/Geometry>

void saveTrajectoryTUM(const CloudPtrPose& KeyPoses, const std::string& fileName);

// 发布局部关键帧map的特征点云
void publishGlobalMap();

// 计算当前帧与前一帧位姿变换，如果变化太小，不设为关键帧，反之设为关键帧
bool saveFrame();
// 添加激光里程计因子
void addOdomFactor();

void saveKeyFramesAndFactor();

void recontructIKdTree();

// 更新因子图中所有变量节点的位姿，也就是所有历史关键帧的位姿，更新里程计轨迹
void correctPoses();

void LoadParams(ros::NodeHandle& nh);
void LoadGTSAM(ros::NodeHandle& nh);


// 回环检测线程
void loopClosureThread();
// rviz展示闭环边
void visualizeLoopClosure();

void LoadParamsLoop(ros::NodeHandle& nh);
// 添加闭环因子
void addLoopFactor();
